 #include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <sstream>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Empty.h>
#include <stdlib.h>
#include <cstdio>
#include <cmath>

// 函数功能：将数据经由串口发送出去
// 入口参数1：[serial::Serial &ser]：     串口类名称
// 入口参数2：[std::string &serial_msg]:  要通过串口发送出去的字符串
int serial_write(serial::Serial &ser, std::string &serial_msg)
{
    ser.write(serial_msg);
    return 0;
}

// 函数功能：将从串口接收到的数据保存到数组中
// 入口参数1：[serial::Serial &ser]：     串口类名称
// 入口参数2：[std::string &serial_msg]:  从串口读取的字符串
int serial_read(serial::Serial &ser, std::string &serial_msg)
{
    serial_msg = ser.read(ser.available());
    return 0;
}

// CRC8
uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
{
    uint8_t crc_u = crc ^ crc_seed;

    for (int i = 0; i < 8; ++i)
    {
        crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
    }

    return crc_u;
}

// CRC8校验
uint8_t crc8(const uint8_t *buf, uint8_t len)
{
    uint8_t crc = 0;

    for (int i = 0; i < len; ++i)
    {
        crc = updateCrc8(buf[i], crc);
    }

    return crc;
}

// 字符转成16进制，a1为数组具体的一位。
std::string Char2Hex(char a1)
{
    std::string str = "";
    int n = a1; // n为a1的ASCII码
    for (int i = 2 * sizeof(char) - 1; i >= 0; i--)
    {
        str += "0123456789ABCDEF"[((n >> i * 4) & 0xF)];
    }
    return str;
}

int main(int argc, char **argv)
{
    setlocale(LC_CTYPE, "zh_CN.utf8"); // 设置编码，防止中文乱码

    // 初始化，节点名为serial_publisher
    ros::init(argc, argv, "serial_publisher");
    // 创建句柄seuNB，用于管理资源
    ros::NodeHandle nh;


    // 实例化一个serial类
    serial::Serial ser;

    // 初始化串口相关设置
    ser.setPort("/dev/ttyUSB2");                               // 设置打开的串口名称:这里打开一个虚拟串口
    ser.setBaudrate(115200);                                   // 设置串口的波特率
    serial::Timeout to = serial::Timeout::simpleTimeout(1000); // 创建timeout
    ser.setTimeout(to);                                        // 设置串口的timeout

    // 打开串口
    try
    {
        ser.open(); // 打开串口
    }

    catch (const std::exception &e)
    {
        ROS_ERROR_STREAM("Unable to open port."); // 打开串口失败，打印日志信息，然后结束程序
        return -1;
    }

    // 判断串口是否成功打开
    if (ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port is opened.\n"); // 成功打开串口，打印日志信息
    }
    else
    {
        return -1; // 打开串口失败，打印日志信息，然后结束程序
    }

    ros::Rate loop_rate(50); // 指定循环频率50
    while (ros::ok())
    {
        // 将数据传入到serial_msg1中
        // 获取缓冲区内的字节数
        size_t n = ser.available();
        uint8_t buffer[1024];
        if (n != 0)
        {
            uint8_t buffer[1024];
            n = ser.read(buffer, n);
            unsigned int crcCode;
            unsigned int number1[15] = {
                buffer[0], buffer[1],   
                buffer[2], buffer[3],   // 编码器位置
                buffer[4], buffer[5],   // 转速计算
                buffer[6], buffer[7],   // 相位计算
                buffer[8], buffer[9],   // 幅值计算
                buffer[10], buffer[11],   // 油门指令
                buffer[12], buffer[13],   // 正弦相位
            };
            int head0 = buffer[0];
            int head1 = buffer[1];
          
            unsigned int number2[2] = {buffer[14]}; // crc8校验
            unsigned int *ptr = NULL;
            ptr = number1;
            crcCode = crc8(buffer, 14);
            number1[15] = crcCode;

            // unsigned int *ptr = NULL;
            // ptr = number1;
            // crcCode = crc8(buffer, 17);
            // number1[18] = crcCode;

            int value1 = crcCode;
            
            int value2 = number2[0];
            
            ROS_INFO_STREAM("crc1："<<value1);
            ROS_INFO_STREAM("crc2："<<value2);

            int speed = buffer[5] << 24 | buffer[4] << 16 | buffer[3] << 8 | buffer[2]; 
            int amplitude = buffer[9] << 24 | buffer[8] << 16 | buffer[7] << 8 | buffer[6]; 
            int phase = buffer[13] << 24 | buffer[12] << 16 | buffer[11] << 8 | buffer[10]; 
                // 编码器位置 

            int m_0 = buffer[0] ;
            int m_1 = buffer[1];
            int m_2 = buffer[2] ;
            int m_3 = buffer[3] ;
            int m_4 = buffer[4];
            int m_5 = buffer[5] ;
            int m_6 = buffer[6];
            int m_7 = buffer[7] ;
            int m_8 = buffer[8] ;
            int m_9 = buffer[9];
            int m_10 = buffer[10] ;
            int m_11 = buffer[11];
            int m_12 = buffer[12] ;
            int m_13 = buffer[13] ;
            int m_14 = buffer[14];
            ROS_INFO_STREAM("0："<<m_0);
            ROS_INFO_STREAM("1："<<m_1);
            ROS_INFO_STREAM("2："<<m_2);
            ROS_INFO_STREAM("3："<<m_3);
            ROS_INFO_STREAM("4："<<m_4);
            ROS_INFO_STREAM("5："<<m_5);
            ROS_INFO_STREAM("6："<<m_6);
            ROS_INFO_STREAM("7："<<m_7);
            ROS_INFO_STREAM("8："<<m_8);
            ROS_INFO_STREAM("9："<<m_9);
            ROS_INFO_STREAM("10："<<m_10);
            ROS_INFO_STREAM("11："<<m_11);
            ROS_INFO_STREAM("12："<<m_12);
            ROS_INFO_STREAM("13："<<m_13);
            ROS_INFO_STREAM("14："<<m_14);
            ROS_INFO_STREAM("15："<<speed);
            ROS_INFO_STREAM("16："<<amplitude);
            ROS_INFO_STREAM("17："<<phase);

          

            loop_rate.sleep();
        }

        
    }
    // 关闭串口
        ser.close();
        return 0;
}
 
